#include"visual_odom/camera.h"
namespace test_visual_odom{

Eigen::Vector3d Camera::pixel2camera(const Eigen::Vector2d& pos_pixel, double depth){
    return Eigen::Vector3d(
            (pos_pixel(0, 0) - cx_) * depth / fx_,
            (pos_pixel(1, 0) - cy_) * depth / fy_,
            depth
    );
}

Eigen::Vector2d Camera::world2pixel(const Eigen::Vector3d &p_w, const SE3 &T_c_w) {
    return camera2pixel(world2camera(p_w, T_c_w));
}

Eigen::Vector3d Camera::world2camera(const Eigen::Vector3d &p_w, const SE3 &T_c_w) {
    return pose_ * T_c_w * p_w;
}

Eigen::Vector2d Camera::camera2pixel(const Eigen::Vector3d &p_c) {
    return Eigen::Vector2d(
            fx_ * p_c(0, 0) / p_c(2, 0) + cx_,
            fy_ * p_c(1, 0) / p_c(2, 0) + cy_
    );
}

}